#include "fill_matrix_lacunae.h"
#include <algorithm>
#include <iostream>
#include <vector>
#include "matrix.h"
using std::vector;
using std::cerr;
using std::max;
using std::min;


// for each non a-c node
// e.g. b
// find max(a-b b-c)
// min of all is missing value
double get_non_ultrametric_lacunae (t_matrix& matrix, unsigned i, unsigned j)
{
	double value = 10000.0;
	
	// get all possible k and l
	vector<unsigned> non_nulls;
	for (unsigned k = 0; k < matrix.size() ; ++k)
	{
		if (k == i || k == j)
			continue;
		if (matrix[i][k] > 5000.0 ||
			matrix[j][k] > 5000.0)
			continue;
		non_nulls.push_back(k);
	}
	
	for (unsigned kk = 0; kk + 1 < non_nulls.size(); ++kk)
		for (unsigned ll = kk + 1; ll < non_nulls.size(); ++ll)
	{
		unsigned k = non_nulls[kk];
		unsigned l = non_nulls[ll];
		if (matrix[k][l] > 5000.0)
			continue;
		double len = max(matrix[i][k] + matrix[j][l], matrix[k][j] + matrix[i][l])
							- matrix[k][l];
		if (len < value && len > 0)
		{
			/*cerr
				<< i << "\t"
				<< j << "\t"
				<< k << "\t"
				<< l << "\t"
				<< len << "\n";*/
			value = len;
		}
	}
	return value;
}


// for each 2 non a-c node
// e.g. b d
// take max(a-b + c-d, b-c + a-d) - b-d
double get_ultrametric_lacunae (t_matrix& matrix, unsigned i, unsigned j)
{
	double value = 10000.0;
	for (unsigned k = 0; k < matrix.size() ; ++k)
	{
		if (k == i || k == j)
			continue;
		if (matrix[i][k] > 5000.0 ||
			matrix[j][k] > 5000.0)
			continue;
		value = min(value, max(matrix[i][k], matrix[j][k]));
	}
	return value;
}

// fill lacunae using
// Landry P.-A., Lapointe F.-J. & Kirsch J.A.W. 1996.
// Estimating phylogenies from lacunose distance matrices:
// additive is superior to ultrametric estimation. Molecular Biology and Evolution 13: 818-823.
//Lapointe F.-J. & Kirsch J.A.W. 1995. Estimating phylogenies from lacunose distance matrices, with special reference to DNA hybridization data. Molecular Biology and Evolution 12: 266-284.
void fill_lacunae(t_matrix& matrix)
{
	for (unsigned i = 0; i < matrix.size() - 1 ; ++i)
		for (unsigned j = i + 1; j < matrix.size(); ++j)
	{
		if (matrix[i][j] < 5000.0)
			continue;
		// try and fill lacunae using non-ultrametric first
		matrix[i][j] =
		matrix[j][i] = get_non_ultrametric_lacunae(matrix, i, j);
		if (matrix[i][j] < 5000.0)
			continue;
		// then try and fill lacunae using non-ultrametric
		matrix[i][j] =
		matrix[j][i] = get_ultrametric_lacunae(matrix, i, j);
	}
}

